using BepuPhysics.Collidables;
using BepuPhysics.Constraints;
using System;
using System.Diagnostics;
using System.Runtime.CompilerServices;

namespace BepuPhysics.CollisionDetection
{
    /// <summary>
    /// 与一对各自由物体控制的两个可碰撞物体相关联的。
    /// </summary>
    public struct TwoBodyHandles
    {
        public int A;
        public int B;
    }

    /// <summary>
    /// 不需要存储任何补充信息的碰撞对的特殊类型。
    /// </summary>
    struct EmptyCollisionCache : IPairCacheEntry
    {
        public int CacheTypeId => -1;
    }

    public struct ContactImpulses1
    {
        public float Impulse0;
    }
    public struct ContactImpulses2
    {
        public float Impulse0;
        public float Impulse1;
    }
    public struct ContactImpulses3
    {
        public float Impulse0;
        public float Impulse1;
        public float Impulse2;
    }
    public struct ContactImpulses4
    {
        public float Impulse0;
        public float Impulse1;
        public float Impulse2;
        public float Impulse3;
    }
    public struct ContactImpulses5
    {
        public float Impulse0;
        public float Impulse1;
        public float Impulse2;
        public float Impulse3;
        public float Impulse4;
    }
    public struct ContactImpulses6
    {
        public float Impulse0;
        public float Impulse1;
        public float Impulse2;
        public float Impulse3;
        public float Impulse4;
        public float Impulse5;
    }
    public struct ContactImpulses7
    {
        public float Impulse0;
        public float Impulse1;
        public float Impulse2;
        public float Impulse3;
        public float Impulse4;
        public float Impulse5;
        public float Impulse6;
    }
    public struct ContactImpulses8
    {
        public float Impulse0;
        public float Impulse1;
        public float Impulse2;
        public float Impulse3;
        public float Impulse4;
        public float Impulse5;
        public float Impulse6;
        public float Impulse7;
    }

    public partial class NarrowPhase<TCallbacks> where TCallbacks : struct, INarrowPhaseCallbacks
    {
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private unsafe void RedistributeImpulses<TContactImpulses>(
            int oldContactCount, int* oldFeatureIds, float* oldImpulses,
            int newContactCount, ref int newFeatureIds, ref TContactImpulses newImpulsesContainer)
        {
            // 将新联系人映射到旧联系人。
            ref var newImpulses = ref Unsafe.As<TContactImpulses, float>(ref newImpulsesContainer);
            // 请注意,下面的指针强制转换实际上不是GC漏洞：
            // 从碰撞批处理程序传递到这里的触点流形和朋友都存储在堆栈或固定的缓冲区中。
            int unmatchedCount = 0;
            for (int i = 0; i < newContactCount; ++i)
            {
                ref var newImpulse = ref Unsafe.Add(ref newImpulses, i);
                // 累积的脉冲不能为负;我们使用负值作为"不匹配"标志。
                newImpulse = -1;
                for (int j = 0; j < oldContactCount; ++j)
                {
                    if (oldFeatureIds[j] == Unsafe.Add(ref newFeatureIds, i))
                    {
                        newImpulse = oldImpulses[j];
                        // 消除旧脉冲,使其不会分配给不匹配的触点。
                        oldImpulses[j] = 0;
                        break;
                    }
                }
                if (newImpulse < 0)
                {
                    ++unmatchedCount;
                }
            }
            // 将丢失的脉冲均匀分布在其余不匹配的触点上。
            if (unmatchedCount > 0)
            {
                float unmatchedImpulse = 0;
                for (int i = 0; i < oldContactCount; ++i)
                {
                    unmatchedImpulse += oldImpulses[i];
                }
                var impulsePerUnmatched = unmatchedImpulse / unmatchedCount;
                for (int i = 0; i < newContactCount; ++i)
                {
                    ref var newImpulse = ref Unsafe.Add(ref newImpulses, i);
                    // 如果我们在第一个循环中将脉冲标记为不匹配,我们可以在这里填充它。
                    if (newImpulse < 0)
                    {
                        // 新脉冲=0;
                        newImpulse = impulsePerUnmatched;
                    }

                }
            }
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        unsafe void RequestAddConstraint<TDescription, TBodyHandles, TContactImpulses>(int workerIndex, int manifoldConstraintType,
            ref CollidablePair pair, PairCacheIndex constraintCacheIndex, ref TContactImpulses newImpulses,
            ref TDescription description, TBodyHandles bodyHandles) where TDescription : unmanaged, IConstraintDescription<TDescription>
        {
            // 请注意,此分支是(WAS？)JIT常量。
            if (typeof(TBodyHandles) != typeof(TwoBodyHandles) && typeof(TBodyHandles) != typeof(int))
            {
                throw new InvalidOperationException("Invalid body handles type; the narrow phase should only use TwoBodyHandles or int.");
            }
            AddConstraint(workerIndex, manifoldConstraintType, ref pair, constraintCacheIndex, ref newImpulses, bodyHandles, ref description);
        }

        public unsafe void UpdateConstraint<TBodyHandles, TDescription, TContactImpulses, TCollisionCache, TConstraintCache>(int workerIndex, ref CollidablePair pair,
            int manifoldTypeAsConstraintType, ref TConstraintCache newConstraintCache, ref TCollisionCache collisionCache,
            ref TDescription description, TBodyHandles bodyHandles)
            where TConstraintCache : unmanaged, IPairCacheEntry
            where TCollisionCache : unmanaged, IPairCacheEntry
            where TDescription : unmanaged, IConstraintDescription<TDescription>
            where TContactImpulses : unmanaged
        {
            var index = PairCache.IndexOf(ref pair);
            if (index >= 0)
            {
                // 上一帧对此对有约束。
                ref var pointers = ref PairCache.GetPointers(index);
                Debug.Assert(pointers.ConstraintCache.Exists, "If a pair was persisted in the narrow phase, there should be a constraint associated with it.");

                var constraintCacheIndex = pointers.ConstraintCache;
                var oldConstraintCachePointer = PairCache.GetOldConstraintCachePointer(index);
                var constraintHandle = *(ConstraintHandle*)oldConstraintCachePointer;
                Solver.GetConstraintReference(constraintHandle, out var constraintReference);
                Debug.Assert(
                    constraintReference.typeBatchPointer != null &&
                    constraintReference.IndexInTypeBatch >= 0 &&
                    constraintReference.IndexInTypeBatch < constraintReference.TypeBatch.ConstraintCount,
                    "Handle-retrieved constraint reference must point to a constraint of expected type, or else something is corrupted.");
                var newImpulses = default(TContactImpulses);
                var accessor = contactConstraintAccessors[constraintReference.TypeBatch.TypeId];
                var oldImpulses = stackalloc float[accessor.ContactCount];
                accessor.GatherOldImpulses(ref constraintReference, oldImpulses);
#if DEBUG
                for (int i = 0; i < accessor.ContactCount; ++i)
                {
                    Debug.Assert(oldImpulses[i] >= 0, "Penetration limit impulses must be nonnegative. Otherwise, something's wrong. Busted gather? Broken constraint?");
                }
#endif
                // 约束缓存中的第一个槽是约束句柄;随后的槽是功能ID。
                RedistributeImpulses(
                    accessor.ContactCount, (int*)oldConstraintCachePointer + 1, oldImpulses,
                    newConstraintCache.CacheTypeId + 1, ref Unsafe.Add(ref Unsafe.As<TConstraintCache, int>(ref newConstraintCache), 1), ref newImpulses);

                if (manifoldTypeAsConstraintType == constraintReference.TypeBatch.TypeId)
                {
                    // 因为旧的约束是同一类型的,所以我们不会删除旧的约束并添加新的约束。这意味着没有延迟的进程
                    // 若要更新约束缓存的约束句柄,请执行以下操作。好消息是,我们已经从预先存在的约束中获得了一个有效的约束句柄。
                    // 它是完全相同的类型,所以我们可以直接覆盖它的属性,而不用担心。
                    // 请注意,我们依赖于存储在约束缓存的前4个字节中的约束句柄。
                    Unsafe.As<TConstraintCache, ConstraintHandle>(ref newConstraintCache) = constraintHandle;
                    PairCache.Update(workerIndex, index, ref pointers, ref collisionCache, ref newConstraintCache);
                    // 存在一个约束,它与流形具有相同的类型。直接应用新的描述和冲动。
                    Solver.ApplyDescriptionWithoutWaking(ref constraintReference, ref description);
                    accessor.ScatterNewImpulses(ref constraintReference, ref newImpulses);
                }
                else
                {
                    // 存在约束,但它是不同类型的。这更加复杂：
                    // 1)必须添加新流形的约束,但在加法器返回时,求解器还不包含该约束。他们被推迟了。
                    // 2)必须删除旧约束。
                    PairCache.Update(workerIndex, index, ref pointers, ref collisionCache, ref newConstraintCache);
                    RequestAddConstraint(workerIndex, manifoldTypeAsConstraintType, ref pair, pointers.ConstraintCache, ref newImpulses, ref description, bodyHandles);
                    ConstraintRemover.EnqueueRemoval(workerIndex, constraintHandle);
                }
            }
            else
            {
                // 没有预先存在的约束;添加新的约束和配对缓存条目。
                // 必须首先创建对高速缓存条目,以便加法器具有放置约束加法的结果的位置。
                var constraintCacheIndex = PairCache.Add(workerIndex, ref pair, ref collisionCache, ref newConstraintCache);
                var newImpulses = default(TContactImpulses);
                // TODO：对于全新的约束,避免脉冲散射会很好;不管怎样,它都将是全零的。值得以后再调查。
                RequestAddConstraint(workerIndex, manifoldTypeAsConstraintType, ref pair, constraintCacheIndex, ref newImpulses, ref description, bodyHandles);
                // 这是约束图中的新连接,因此我们必须检查是否有任何涉及的实体处于非活动状态。
                // 请注意,这仅在两个碰撞器都是物体时才有可能。如果只有一个对撞机是一个物体,那么它必须是激活的,否则这对对撞机永远不会被测试。
                if (typeof(TBodyHandles) == typeof(TwoBodyHandles))
                {
                    ref var twoBodyHandles = ref Unsafe.As<TBodyHandles, TwoBodyHandles>(ref bodyHandles);
                    ref var locationA = ref Bodies.HandleToLocation[twoBodyHandles.A];
                    ref var locationB = ref Bodies.HandleToLocation[twoBodyHandles.B];
                    // 这两个选项中只有一个可以处于非活动状态。
                    if (locationA.SetIndex != locationB.SetIndex)
                    {
                        ref var overlapWorker = ref overlapWorkers[workerIndex];
                        overlapWorker.PendingSetAwakenings.Add(locationA.SetIndex > 0 ? locationA.SetIndex : locationB.SetIndex, overlapWorker.Batcher.Pool);
                    }
                }
            }
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static int ExtractContactConstraintBodyCount(int contactConstraintTypeId)
        {
            Debug.Assert(contactConstraintTypeId >= 0 && contactConstraintTypeId < PairCache.CollisionConstraintTypeCount);
            // [0,3]和[8,14]是一体。否则,两个。
            // 如果重要的话可能会比这更聪明一点。
            return contactConstraintTypeId <= 3 || (contactConstraintTypeId >= 8 && contactConstraintTypeId <= 14) ? 1 : 2;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        static int GetConvexConstraintTypeId<TBodyHandles>(int contactCount)
        {
            // 凸面约束：
            // 1-4个联系人：0x3
            // 2主体(未设置为1主体)：0x4
            // 因此：
            // 凸面一个实体约束,接触计数1到4：[0,3]
            // 凸面两实体约束,接触计数1到4：[4,7]
            int manifoldTypeAsConstraintType;
            Debug.Assert(contactCount > 0);
            manifoldTypeAsConstraintType = (contactCount - 1);
            if (typeof(TBodyHandles) == typeof(TwoBodyHandles))
                manifoldTypeAsConstraintType |= 0x4;
            return manifoldTypeAsConstraintType;
        }

        // TODO：如果最终更改了NarrowPhasePendingConstraintAdds和PairCache硬编码类型处理,则也应该更改此类型。这太傻了。
        unsafe void UpdateConstraintForManifold<TContactManifold, TCollisionCache, TBodyHandles>(
            int workerIndex, ref CollidablePair pair, ref TContactManifold manifold, ref TCollisionCache collisionCache, ref PairMaterialProperties material, TBodyHandles bodyHandles)
            where TCollisionCache : unmanaged, IPairCacheEntry
        {
            // 请注意,此功能有两个职责：
            // 1)创建应表示新流形的约束描述。
            // 2)使用该描述添加该约束(或更新现有约束),根据需要更新任何累积的脉冲。
            int manifoldTypeAsConstraintType;
            if (typeof(TContactManifold) == typeof(ConvexContactManifold))
            {
                ref var convexManifold = ref Unsafe.As<TContactManifold, ConvexContactManifold>(ref manifold);
                manifoldTypeAsConstraintType = GetConvexConstraintTypeId<TBodyHandles>(convexManifold.Count);
            }
            else
            {
                Debug.Assert(typeof(TContactManifold) == typeof(NonconvexContactManifold));
                ref var nonconvexManifold = ref Unsafe.As<TContactManifold, NonconvexContactManifold>(ref manifold);
                Debug.Assert(nonconvexManifold.Count > 0);
                if (nonconvexManifold.Count == 1)
                {
                    // 没有"非凸"的一个接触约束。
                    manifoldTypeAsConstraintType = GetConvexConstraintTypeId<TBodyHandles>(1);
                }
                else
                {
                    // 非凸面约束：
                    // 要跳过凸约束,请从8开始
                    // 非凸面约束的范围为2到8个接触(1个接触约束被视为凸面)
                    // 一个实体约束具有较低的ID
                    // 因此：
                    // 非凸一体式约束,接触计数为2到8：[8,14]
                    // 非凸两体约束,接触计数为2到8：[15,21]
                    manifoldTypeAsConstraintType = 8 + (nonconvexManifold.Count - 2);
                    if (typeof(TBodyHandles) == typeof(TwoBodyHandles))
                        manifoldTypeAsConstraintType += 7;
                }
            }
            contactConstraintAccessors[manifoldTypeAsConstraintType].UpdateConstraintForManifold(this, manifoldTypeAsConstraintType, workerIndex, ref pair, ref manifold, ref collisionCache, ref material, bodyHandles);
        }

        public unsafe void UpdateConstraintsForPair<TContactManifold, TCollisionCache>(int workerIndex, ref CollidablePair pair, ref TContactManifold manifold, ref TCollisionCache collisionCache)
            where TCollisionCache : unmanaged, IPairCacheEntry
            where TContactManifold : unmanaged, IContactManifold<TContactManifold>
        {
            // 请注意,在报告之前,我们不会检查该对是否处于两个静力学之间。假设是,如果最初的宽相位对过滤允许这样的对
            // 为了达到这一点,用户可能想要接收有关所产生的触点歧管的一些信息。
            // 也就是说,这样的一对无论如何都不能生成约束-约束必须始终至少涉及一个实体。
            var aMobility = pair.A.Mobility;
            var bMobility = pair.B.Mobility;
            Debug.Assert(aMobility != CollidableMobility.Static, "The broad phase should not generate static-static pairs ever, and any static collidable should be in slot B.");
            bool allowConstraint;
            allowConstraint = Callbacks.ConfigureContactManifold(workerIndex, pair, ref manifold, out var pairMaterial) && manifold.Count > 0;
            if (allowConstraint &&
                // 请注意,即使回调说‘是的,一定要为那些创建一个约束’,在两个非动态之间生成约束也是没有意义的。
                // 这只会在计算有效质量时产生一堆NaN。
                (aMobility == CollidableMobility.Dynamic || bMobility == CollidableMobility.Dynamic))
            {
                if (bMobility != CollidableMobility.Static)
                {
                    // 两具身体。
                    Debug.Assert(pair.A.Mobility != CollidableMobility.Static && pair.B.Mobility != CollidableMobility.Static);
                    var bodyHandles = new TwoBodyHandles { A = pair.A.BodyHandle.Value, B = pair.B.BodyHandle.Value };
                    UpdateConstraintForManifold(workerIndex, ref pair, ref manifold, ref collisionCache, ref pairMaterial, bodyHandles);
                }
                else
                {
                    // 两个可碰撞的物体之一是静电。
                    Debug.Assert(pair.A.Mobility != CollidableMobility.Static && pair.B.Mobility == CollidableMobility.Static);
                    UpdateConstraintForManifold(workerIndex, ref pair, ref manifold, ref collisionCache, ref pairMaterial, pair.A.BodyHandle.Value);
                }
                // 如果新歧管中没有触点,则该对将处于陈旧状态。它将通过过时的删除后处理过程删除。
            }
        }

    }
}
